#include "ros/ros.h"
#include "std_msgs/Float32MultiArray.h"
#include <vector>
#include <cmath>

#define step_length 0.05 // 步长，越小分点越细
#define dtheta (M_PI / 90)  // 圆弧段每次步进的角度（约2度）
bool cnt = false;
int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "path_publisher");
    ros::NodeHandle nh;

    ros::Publisher pub = nh.advertise<std_msgs::Float32MultiArray>("/mpc_route", 10); // 发布路径点

    // 设定路径点ABCDEFGH
    std::vector<std::pair<float, float>> waypoints = {
        {0.0, 0.0},   // A
        {1.8, 0.0},  // B
        {2.4, 0.3}, // C
        {3.0, 0.3}, // D
   	    {3.6, 0.0}, 
        {4.6, 0.0},
        {5.6, 0.0},      
        {6.3, 0.0},  // E
        {6.7, -0.2},
        {6.9, -0.6}, // G
        {6.9, -1.4},
        {6.9, -2.4},
        {6.9, -3.4},
        {6.9, -4.4},
        {6.9, -5.4},
        {6.9, -6.4}
    

    };

    std_msgs::Float32MultiArray route;

    for (size_t i = 0; i < waypoints.size() - 1; ++i)
    {
        float start_x = waypoints[i].first;
        float start_y = waypoints[i].second;
        float end_x = waypoints[i + 1].first;
        float end_y = waypoints[i + 1].second;

        // 特判 F->G 的圆弧段（点 F 是 waypoints[5], G 是 waypoints[6]）
      
       
            float dX = end_x - start_x;
            float dY = end_y - start_y;

            float theta = 0; // 朝向角

            int step = std::max(fabs(dX / step_length), fabs(dY / step_length));
            float dx = dX / step;
            float dy = dY / step;

            for (int j = 0; j <= step; ++j)
            {
                float x = start_x + j * dx;
                float y = start_y + j * dy;
                route.data.push_back(x);
                route.data.push_back(y);
                route.data.push_back(theta);
            }
        
    }
    ros::Rate rate(1); // 每秒发布1次
    while (ros::ok())
    {
   
      	pub.publish(route);
 
        rate.sleep();
    }

    return 0;
}
